#include "facedir.h"

extern char Model_SMAT[2][100];


void CFacedir::facedir()
{
    //se pasa el vector de rotación de coordenadas de una cámara a la otra
    ChangeView(vectR, translationR, &calibration_paramsL, vectRl, translationRl, rot);

    //media de los dos vectores
    //media de los dos vectores
    cvAddWeighted(vectL, (float)inliersI/(float)(inliersI+inliersD), 
		  vectRl, (float)inliersD/(float)(inliersD+inliersI), 0, 
		  vect_media_L);
    //cvAddWeighted(vectL, errorI/(errorI+errorD), vect, errorD/(errorI+errorD), 0, vectL);
    cvAddWeighted(translationL, (float)inliersI/(float)(inliersI+inliersD), 
		  translationRl, (float)inliersD/(float)(inliersD+inliersI), 0, 
		  translation_media_L);
    if(frame>=0)
    {
	if (inliersI < MIN_INLIERS &&   inliersD > MIN_INLIERS)
	{
	    //usa el valor de la otra cámara, de la derecha
	    ChangeView(vectR, translationR, &calibration_paramsL, vectL, translationL, rot);
	    cvProjectPoints2(points3D, vectL, translationL, calibration_paramsL.KK, NULL, points2D_reprojectI);
	    cvCopy(vectL, vect_media_L);
	    cvCopy(translationL, translation_media_L);
	    cvCopy(vectL, vectRl);
	    cvCopy(translationL, translationRl);

	    cvSet(maskoutI, cvScalar(255), maskL);
	    smatI->remove_last_added(maskoutI);
		
	    //update the value of the outlier points with the reprojected value
	    cvCopy(points2D_reprojectI, pointsID[0], 0);
	}
	else
	{
	    if(inliersI > MIN_INLIERS2)
	    {
		if (frame == 128)
		    key=0;
		cvProjectPoints2(points3D, vectL, translationL, calibration_paramsL.KK, NULL, points2D_reprojectI);
		    
		cvCmpS(maskoutI, 0, maskoutI, CV_CMP_EQ);//invert the mask
		cvAnd(maskoutI, maskL, maskoutI);
		smatI->remove_last_added(maskoutI);
		
		//update the value of the outlier points with the reprojected value
		cvRepeat(maskoutI, maskoutI2);
		cvCopy(points2D_reprojectI, pointsID[0], maskoutI2);
	    }
	    else
	    {
		cvSet(maskoutI, cvScalar(255), maskL);
		smatI->remove_last_added(maskoutI);
	    }

	}
    }


    if(frame>=0 && inliersD)
    {
	if (inliersD < MIN_INLIERS &&   inliersI > MIN_INLIERS)
	{
	    //usa el valor de la otra cámara, de la izquierda
	    ChangeView(vectL, translationL, &calibration_paramsR, vectR, translationR, rot);
	    cvProjectPoints2(points3D, vectR, translationR, calibration_paramsR.KK, NULL, points2D_reprojectD);
	    cvCopy(vectL, vect_media_L);
	    cvCopy(translationL, translation_media_L);
	    cvCopy(vectL, vectRl);
	    cvCopy(translationL, translationRl);
		
	    cvSet(maskoutD, cvScalar(255), maskR);
	    smatD->remove_last_added(maskoutD);
		
	    //update the value of the outlier points with the reprojected value
	    cvCopy(points2D_reprojectD, pointsID[1], 0);
	}
	else
	{
	    if(inliersD > MIN_INLIERS2)
	    {
		cvProjectPoints2(points3D, vectR, translationR, calibration_paramsR.KK, NULL, points2D_reprojectD);
		
		cvCmpS(maskoutD, 0, maskoutD, CV_CMP_EQ);//invert the mask
		cvAnd(maskoutD, maskR, maskoutD);
		smatD->remove_last_added(maskoutD);
		
		//update the value of the outlier points with the reprojected value
		cvRepeat(maskoutD, maskoutD2);
		cvCopy(points2D_reprojectD, pointsID[1], maskoutD2);
	    }
	    else
	    {
		cvSet(maskoutD, cvScalar(255), maskR);
		smatD->remove_last_added(maskoutD);
	    }
	}
    }

    ChangeView(vect_media_L, translation_media_L, &calibration_paramsR, 
	       vect_media_R, translation_media_R, rot);
    ChangeView(vectR, translationR, &calibration_paramsL, vectRl, translationRl, rot);

}


void CFacedir::facedir_control()
{
    do
    {
	printf(CRED "inliersI : %d, error: %f" CNORMAL "\n", inliersI, params_left.error);
	CvMat *vect2 = cvCreateMat(3, 1, CV_32FC1);
	cvScale(vectL, vect2, 180.0/CV_PI);
	printf("vectL: ");
	print_arr(vect2, true, "%1.2f,");
	printf("translationL: ");
	print_arr(translationL, true, "%3f, ");
	print_arr(params_left.maskout, 1, "%3.0f, ");
	print_arr(masktemp, 1, "%3.0f, ");
	cvReleaseMat(&vect2);

	cvConvertImage(params_left.img, clonek);
	DibujarModelo(clonek, pointsID[0], &calibration_paramsL, translation_media_L, 
		      vect_media_L,lines, params_left.mask_show, params_left.maskout, CV_RGB(00,60,200), -1);
	DibujarModelo3D(clonek, params_left.points3D,  &calibration_paramsL, translation_media_L, 
			vect_media_L,
			lines, params_left.mask_show,  params_left.maskout, CV_RGB(255,0,0), -1, false);
	cvShowImage(Model_SMAT[0], clonek);
	cvShowImage("SMAT", display);
	
	np_to_show_imgs2 = np_to_show_imgs;

	modificando = 0;
	while ((key = bucle_cvWaitKey(&wait)) > 0)
	{
	    if (!(key & 0xFF0000))
	    {
		switch (key)
		{
		case 'I':
		case 'i':
		    print_model_state(smatD);	    
		    break;
		case  0xFF51: //BACK ARROW
		    break;
		case  0xFF53: //NEXT ARROW
		    cvShowImage("SMAT", buffer_display_smat[np_to_show_imgs2]);
		    break;
		default:
		    printf("Undefined key %d (%X)\n", key, key);
		}
	    }
	    else
	    {
		key = 1;
		wait=0;
		break;
	    } //if (key & 0xFF0000)
	}  //while ((key = bucle_cvWaitKey(&wait)) > 0)
	if(key < 0)
	{
	    quit = true;
	    params_left.quit = true;
	}
    } while (modificando && key > 0);

    if (modificado)
    {
	ChangeView(vect_media_L, translation_media_L, &calibration_paramsR, 
		   vect_media_R, translation_media_R, rot);

	cvCopy(vect_media_L, vectL);
	cvCopy(translation_media_L, translationL);
	cvCopy(vect_media_R, vectR);
	cvCopy(translation_media_R, translationR);

	modificado =0;
	printf("-- modificado --\n");
	fflush(stdout);

 	cvProjectPoints2(params_left.points3D, vect_media_L, translation_media_L, 
			 calibration_paramsL.KK, NULL, points2D_reprojectI);
	//update the value of the outlier points with the reprojected value
	cvCopy(points2D_reprojectI, pointsID[0]);

 	cvProjectPoints2(params_right.points3D, vect_media_R, translation_media_R, 
			 calibration_paramsR.KK, NULL, points2D_reprojectD);
	//update the value of the outlier points with the reprojected value
	cvCopy(points2D_reprojectD, pointsID[1]);
    }

    if (frame > 0)
    {	
	printf("Save video?\n");
	if (save_video)
	{
	    //write results
	    if (0)
	    {
		IplImage *final2 = cvCreateImage(cvGetSize(display), IPL_DEPTH_8U, 3);
		cvConvertImage(display, final2);

		int w = 500;
		int h = (int)((float)final2->height / (float) final2->width * (float)w); 
		cvSetImageROI(clonek, cvRect(0, cvGetSize(clonek).height-h, w, h));
		//cvResize(final2, clonek, CV_INTER_CUBIC);
		cvConvertImage(display, final2);
		cvSetImageROI(clonek, cvRect(0, 800-h, w, h));
		cvResize(final2, clonek, CV_INTER_CUBIC);
		cvResetImageROI(clonek); 

		cvReleaseImage(&final2);
	    }
	    //cvWriteFrame(video_results_FaceDir.get(), clonek);
	    if (videofile)
		fwrite(clonek->imageData, clonek->imageSize, 1, videofile);


	    fprintf(fout, "%d %f %f %f %f %f %f\n",
		    //imgs->hleft->nframe, 
		    frame2,
		    cvmGet(vect_media_L,0,0), cvmGet(vect_media_L,1,0),cvmGet(vect_media_L,2,0),
		    cvmGet(translation_media_L,0,0), cvmGet(translation_media_L,1,0),
		    cvmGet(translation_media_L,2,0));
	    fflush(fout);

	    printf("---------------- saving frame %d --------------\n", frame);
	}
    }
}
 
